/*
 * main.c
 */
#include "System\F2806x_Device.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <cmath>
#include "Drivers\debugLED.h"
#include "Timer.h"
#include "CANDriver.h"
#include "Datafile.h"
#include "Drivers\UARTDriverDBG.h"
#include "Drivers\UARTDriverB.h"
#include "Controller\Ctrl.h"
#include "LLConverter.h"

extern "C" void InitSysCtrl(void);

// flash support
#define FLASH 1
extern "C" void InitFlash(void);
extern "C" void MemCopy(Uint16 *SourceAddr, Uint16* SourceEndAddr, Uint16* DestAddr);
extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart;

#define IO_BASE 	0x010000
#define SERVO_BASE 	0x020000
#define IMU_BASE 	0x030000
#define CAN_BASE 	0x040000

// Drivers
DebugLED ledDebug;

// Components
Timer timer;
Timer timerINT;
CANDriver canDrv;
SDataFile df;
UARTDriverB uartB;
UARTDriverDBG uartDBG;
CtrlModelClass ctrlMdl;
LLConverter llConv;


// main step time
#define STEPTIME 0.01 /* standard steptime, 100 Hz */

bool GetNextSentence(char* buffer, int count);
char g_GlobalSerBuffer[200+1];
int g_GlobalSerIndex = 0;

float64 g_Nold[10];
float64 g_Eold[10];

// hngr
#define X0LONG 16.07362102
#define X0LAT 45.80594310

// jel
//#define X0LONG 16.100529
//#define X0LAT 45.811312

// kanal1
//#define X0LONG 15.9223277
//#define X0LAT 45.7453999


int main(void)
{
	//=================================
	//	INITIALISATION - General
	//=================================
	InitSysCtrl();	// Device Life support & GPIO mux settings

	// init debug LED
	ledDebug.Init();

	// init Serial
	uartDBG.Init(57600);
	uartDBG.SendData("XBeeBoard v3.3 - Q10 Edition\n\r");

// Only used if running from FLASH
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The  RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
	MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)

	// Init Components
	timer.Init();
	timerINT.Init();
	canDrv.Init();
	uartB.Init(57600);
	ctrlMdl.initialize();
	llConv.Init(X0LAT, X0LONG);

	// Reset DataFile
	memset(&df, 0, sizeof(df));
	memset(&g_GlobalSerBuffer, 0, sizeof(g_GlobalSerBuffer));
	memset(g_Nold, 0, sizeof(g_Nold));
	memset(g_Eold, 0, sizeof(g_Eold));

	// initial value for target
	df.targetGpsLatitude = X0LAT;
	df.targetGpsLongitude = X0LONG;

	// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

//=================================
//	Endless LOOP
//=================================
	for(;;)  //infinite loop
	{
		timer.BeginPeriod();
		df.Counter++;

		// Process UART Inputs/Commands
		char buffer[200];
		Uint32 count = uartB.GetData(buffer);
		bool executed = GetNextSentence(buffer, count);

		// Receive CAN Inputs/Commands
		float received;
		canDrv.ReadCANMBOX(0, &df.BattVoltage);
		canDrv.ReadCANMBOX(1, &df.GyroX);
		canDrv.ReadCANMBOX(2, &df.GyroY);
		canDrv.ReadCANMBOX(3, &df.GyroZ);
		canDrv.ReadCANMBOX(4, &df.Roll);
		canDrv.ReadCANMBOX(5, &df.Pitch);
		canDrv.ReadCANMBOX(6, &df.Yaw);
		canDrv.ReadCANMBOX(7, &df.MagX);
		canDrv.ReadCANMBOX(8, &df.MagY);
		canDrv.ReadCANMBOX(9, &df.MagZ);
		canDrv.ReadCANMBOX(12, &df.SonicCM);
		canDrv.ReadCANMBOX(19, &df.IRFarCM);

		// GPS
		canDrv.ReadCANMBOX(13, &df.gpsLongitude);
		canDrv.ReadCANMBOX(14, &df.gpsLatitude);
		canDrv.ReadCANMBOX(21, &df.gpsAltitude);
		df.gpsReceived = canDrv.ReadCANMBOX(30, &df.gpssatCount);

		// calculate velocity
		if( df.gpsReceived )
		{
			float64 dN, dE;
			llConv.LLToFlatEarth(df.gpsLatitude, df.gpsLongitude, dN, dE );

			for(int i=(10-2); i>=0; i--)
			{
				g_Nold[i+1] = g_Nold[i];
				g_Eold[i+1] = g_Eold[i];
			}
			g_Nold[0] = dN;
			g_Eold[0] = dE;

			float64 r = 1/6.0;
			float64 velN = r*(g_Nold[0]+g_Nold[1]+g_Nold[2]-g_Nold[3]-g_Nold[4]-g_Nold[5]);
			float64 velE = r*(g_Eold[0]+g_Eold[1]+g_Eold[2]-g_Eold[3]-g_Eold[4]-g_Eold[5]);

			df.VelX = (float)velN;
			df.VelY = (float)velE;

			// clip errors
			if( fabs(df.VelX) > 200 ) df.VelX = 0;
			if( fabs(df.VelY) > 200 ) df.VelY = 0;

			df.XeX = (float)dN;
			df.XeY = (float)dE;

			// convert target
			float64 dNRef, dERef;
			llConv.LLToFlatEarth(df.targetGpsLatitude, df.targetGpsLongitude, dNRef, dERef );
			df.XeXRef = dNRef;
			df.XeYRef = dERef;
		}



		canDrv.ReadCANMBOX(31, &df.AbsPressurePa);

		if( canDrv.ReadCANMBOX(10, &received) ) df.Mode = (Uint32)received;
		if( canDrv.ReadCANMBOX(11, &received))
		{
			if(received == 1)
			{
				df.AckError = true; // must be autocleared!
				df.Mode = 0;
			}
		}

		// CAN Control - update only on receive
		if( canDrv.ReadCANMBOX(15, &received) )
		{
			df.JoyRollRef = received;
			df.KillCounter  = 200;
		}
		if( canDrv.ReadCANMBOX(16, &received ) ) df.JoyPitchRef = received;
		if( canDrv.ReadCANMBOX(17, &received ) ) df.JoyYawRef = received;
		if( canDrv.ReadCANMBOX(18, &received ) ) df.JoyThrottleRef = received;

		// fix problems
		if( df.AbsPressurePa < 50000 ) df.AbsPressurePa = 100000;

		// Run controller
		ctrlMdl.Ctrl_U.Mode = df.Mode;
		ctrlMdl.Ctrl_U.AckErrr = df.AckError;
		df.AckError = false; // clear ACK
		// joystick/input
		ctrlMdl.Ctrl_U.Roll = df.JoyRollRef;
		ctrlMdl.Ctrl_U.Pitch = df.JoyPitchRef;
		ctrlMdl.Ctrl_U.Yaw = df.JoyYawRef;
		ctrlMdl.Ctrl_U.Throttle = df.JoyThrottleRef;
		// RPY/Omega/mag
		ctrlMdl.Ctrl_U.RPY[0] = df.Roll;
		ctrlMdl.Ctrl_U.RPY[1] = df.Pitch;
		ctrlMdl.Ctrl_U.RPY[2] = df.Yaw;
		ctrlMdl.Ctrl_U.omega[0] = df.GyroX;
		ctrlMdl.Ctrl_U.omega[1] = df.GyroY;
		ctrlMdl.Ctrl_U.omega[2] = df.GyroZ;
		ctrlMdl.Ctrl_U.MagXYZ[0] = df.MagX;
		ctrlMdl.Ctrl_U.MagXYZ[1] = df.MagY;
		ctrlMdl.Ctrl_U.MagXYZ[2] = df.MagZ;
		// sonic/pressures
		ctrlMdl.Ctrl_U.SonicM = df.SonicCM/100;
		ctrlMdl.Ctrl_U.PressureAbs = df.AbsPressurePa;
		// converted GPS coordinates/velocities
		ctrlMdl.Ctrl_U.FlatVe[0] = df.VelX;
		ctrlMdl.Ctrl_U.FlatVe[1] = df.VelY;
		ctrlMdl.Ctrl_U.FlatXe[0] = df.XeX;
		ctrlMdl.Ctrl_U.FlatXe[1] = df.XeY;
		ctrlMdl.Ctrl_U.XeXRef = df.XeXRef;
		ctrlMdl.Ctrl_U.XeYRef = df.XeYRef;

		ctrlMdl.step(); // STEP

		df.ActualMode = ctrlMdl.Ctrl_Y.ActualMode;
		df.Failed = ctrlMdl.Ctrl_Y.Failed;
		df.PWM1 = ctrlMdl.Ctrl_Y.PWM1;
		df.PWM2 = ctrlMdl.Ctrl_Y.PWM2;
		df.PWM3 = ctrlMdl.Ctrl_Y.PWM3;
		df.PWM4 = ctrlMdl.Ctrl_Y.PWM4;
		df.MagYaw = ctrlMdl.Ctrl_Y.MagYaw;
		df.AltF = ctrlMdl.Ctrl_Y.AltF;
		df.VertSpeed = ctrlMdl.Ctrl_Y.VertSpeed;

		// Kill switch/watch dog code, DO NOT KILL, REDUCE POWER (df.JoyThrottleRef)
		if( df.Failed ) df.KillCounter = 0; // kill
		if( df.KillCounter > 0 ) df.KillCounter--;
		else
		{
			// KILL
			df.PWM1 = 1000;
			df.PWM2 = 1000;
			df.PWM3 = 1000;
			df.PWM4 = 1000;

			// reduce thrust!
			//if( df.JoyThrottleRef > 0.36 )
			//{
			//	df.JoyThrottleRef = 0.35; // XXX - CHECK THIS!!!
			//}
		}

		// Send Can Commands! (100 Hz)
		canDrv.SendMBOX(23, df.PWM1);
		canDrv.SendMBOX(24, df.PWM2);
		canDrv.SendMBOX(25, df.PWM3);
		canDrv.SendMBOX(26, df.PWM4);

		// send at 100Hz/100 = 10Hz
		if( (df.Counter % 10) == 0)
		{
			canDrv.SendMBOX(20, df.LoopTimeMS);
			//canDrv.SendMBOX(21, df.MissionTime);
			canDrv.SendMBOX(22, df.LoopTimeMSMAX);

			canDrv.SendMBOX(28, (float)df.ActualMode);
			canDrv.SendMBOX(29, (float)df.Failed);
		}
		// send at 100Hz/10 = 10Hz
		if( (df.Counter % 10) == 0)
		{
			// send status
			char toSend[200];
			sprintf(toSend, "$DATA,%ld,%d,%0.2f,%0.3f,%0.3f,%0.3f,%0.0f,%0.2f,%0.2f,%ld\n\r", df.Counter, (int)df.ActualMode, df.BattVoltage, df.Roll, df.Pitch, df.MagYaw, df.SonicCM, df.AltF, df.VertSpeed, df.KillCounter );
			uartB.QueueData(toSend);
		}
		// send GPS data (~5Hz, synced to GPS)
		if( df.gpsReceived == true )
		{
			char toSend[200];
			sprintf(toSend, "$GPS,%0.3f,%0.3f,%d,%0.8Lf,%0.8Lf,%0.3f,%0.8Lf,%0.8Lf\n\r", df.VelX, df.VelY, (int)df.gpssatCount, df.gpsLongitude, df.gpsLatitude, df.gpsAltitude, df.targetGpsLongitude, df.targetGpsLatitude );
			uartB.QueueData(toSend);
		}


		// Blink LED
		if( (df.Counter % 20) == 0)
		{
			ledDebug.Toggle(DebugLED::GREEN);
		}

		//-----------------------------------------------------------
		// finish loop
		df.LoopTimeMS = timer.ElapsedTime() * 1000; // get looptime (ms)
		if(df.LoopTimeMSMAX < df.LoopTimeMS) df.LoopTimeMSMAX = df.LoopTimeMS;

		while( !timer.HasElapsed(STEPTIME) )
		{
			// do something...
		};
		df.MissionTime += timer.ElapsedTime();
	}
}

bool GetNextSentence(char* buffer, int count)
{
	bool exe = false;
	for(int i=0; i!=count; i++)
	{
		if( buffer[i] == '$' || g_GlobalSerIndex >= 200)
		{
			// restart
			g_GlobalSerIndex = 0;
		}

		g_GlobalSerBuffer[g_GlobalSerIndex] = buffer[i];
		g_GlobalSerIndex++;

		if( buffer[i] == '\r' )
		{
			exe = true;
			g_GlobalSerBuffer[g_GlobalSerIndex] = 0; // null terminate

			// parse command
			char* token = strtok( g_GlobalSerBuffer, ",");

			// 1. check if valid
			if( !strcmp(token, "$REF") )
			{
				float joyrollref = -10;
				float joypitchref = -10;
				float joyyawref = -10;
				float joythrottleref = -10;

				// 2. parse
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					// roll ref
					joyrollref = atof(token);
				}
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					// pitch ref
					joypitchref = atof(token);
				}
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					// yaw ref
					joyyawref = atof(token);
				}
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					// throttle ref
					joythrottleref = atof(token);
				}

				// 3. check results
				if( (joyrollref > -1.1 && joyrollref < 1.1)  &&
						(joypitchref > -1.1 && joypitchref < 1.1)  &&
						(joyyawref > -1.1 && joyyawref < 1.1)  &&
						(joythrottleref > -0.1 && joythrottleref < 1.1) )
				{
					// 4. update datafile
					df.JoyRollRef = joyrollref;
					df.JoyPitchRef = joypitchref;
					df.JoyYawRef = joyyawref;
					df.JoyThrottleRef = joythrottleref;
					df.KillCounter = 200; // restart kill counter
				}
			}
			else if( !strcmp(token, "$MODE"))
			{
				int mode = 0;

				// 2. parse
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					mode = atoi(token);
				}

				// 3. check results
				if( mode >= 0 && mode < 10)
				{
					// 4. update datafile
					df.Mode = mode;
				}
			}
			// TARGET
			else if( !strcmp(token, "$TGT"))
			{
				// 2. parse
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					float64 num;
					sscanf(token, "%Lf", &num );
					// target latitude
					if( num >= 44 && num <= 46.5) // sanity check
					{
						df.targetGpsLatitude = num;
					}
				}
				token = strtok(NULL, ",");
				if( token != NULL )
				{
					float64 num;
					sscanf(token, "%Lf", &num );
					// target longitude
					if( num >= 14 && num <= 18) // sanity check
					{
						df.targetGpsLongitude = num;
					}
				}
			}
		}
	}

	return exe;
}

// SETUP BOXES
void CANSetupBoxes(CANDriver* ptr)
{
	// RX
	ptr->SetupMBOX(0,  IO_BASE+0x000311, 8, false); // Voltage
	ptr->SetupMBOX(1,  IMU_BASE+0x000300, 8, false); // GyroX
	ptr->SetupMBOX(2,  IMU_BASE+0x000301, 8, false); // GyroY
	ptr->SetupMBOX(3,  IMU_BASE+0x000302, 8, false); // GyroZ
	ptr->SetupMBOX(4,  IMU_BASE+0x000310, 8, false); // Roll
	ptr->SetupMBOX(5,  IMU_BASE+0x000311, 8, false); // Pitch
	ptr->SetupMBOX(6,  IMU_BASE+0x000312, 8, false); // Yaw
	ptr->SetupMBOX(7,  IMU_BASE+0x000306, 8, false); // MagX
	ptr->SetupMBOX(8,  IMU_BASE+0x000307, 8, false); // MagY
	ptr->SetupMBOX(9,  IMU_BASE+0x000308, 8, false); // MagZ

	ptr->SetupMBOX(12,  IO_BASE+0x000300, 8, false); // Sonic
	ptr->SetupMBOX(19,  IO_BASE+0x000302, 8, false); // IRFar
	ptr->SetupMBOX(13,  IMU_BASE+0x000330, 8, false); // GPS Longitude
	ptr->SetupMBOX(14,  IMU_BASE+0x000331, 8, false); // GPS Latitude
	ptr->SetupMBOX(21,  IMU_BASE+0x000332, 8, false); // GPS Alt // XXX-TotalTime
	ptr->SetupMBOX(30,  IMU_BASE+0x000333, 8, false); // GPS SatNum
	ptr->SetupMBOX(31,  IMU_BASE+0x000320, 8, false); // Abs. pressure

	// RX Commands/Refs
	ptr->SetupMBOX(10,  CAN_BASE+0x000100, 8, false); // Mode (X)
	ptr->SetupMBOX(11,  CAN_BASE+0x000101, 8, false); // AckError (X)

	ptr->SetupMBOX(15,  CAN_BASE+0x000110, 8, false); // JoyRollRef (X)
	ptr->SetupMBOX(16,  CAN_BASE+0x000111, 8, false); // JoyPitchRef (X)
	ptr->SetupMBOX(17,  CAN_BASE+0x000112, 8, false); // JoyYawRef (X)
	ptr->SetupMBOX(18,  CAN_BASE+0x000113, 8, false); // JoyThrottleRef (X)

	// TX
	ptr->SetupMBOX(20,  CAN_BASE+0x000350, 8, true); // Loop time
	//ptr->SetupMBOX(21,  CAN_BASE+0x000351, 8, true); // TotalTime
	ptr->SetupMBOX(22,  CAN_BASE+0x000352, 8, true); // LooptimeMAX

	ptr->SetupMBOX(23,  IO_BASE+0x000120, 8, true); // PWM1
	ptr->SetupMBOX(24,  IO_BASE+0x000121, 8, true); // PWM2
	ptr->SetupMBOX(25,  IO_BASE+0x000122, 8, true); // PWM3
	ptr->SetupMBOX(26,  IO_BASE+0x000123, 8, true); // PWM4

	ptr->SetupMBOX(28,  CAN_BASE+0x000400, 8, true); // Mode real (X)
	ptr->SetupMBOX(29,  CAN_BASE+0x000401, 8, true); // Fail (X)
}
